import math
import numpy as np
class PID:
    def __init__(self, kp, ki, kd):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.last_err = 0
        self.integral = 0
    def calculate(self,current:float=0.0, target:float=0.0 ,err_threshold:float=0.0,integral_threshold:float=0.0):
        err = current
        # Limit the err
        # Stop integral
        if abs(err) > err_threshold:
            beta = 0
            err = err_threshold if err > 0 else -err_threshold
        else:
            beta = 1

        # Integral separation
        self.integral += err
        if self.integral > integral_threshold:
            self.integral = integral_threshold
        elif self.integral < -integral_threshold:
            self.integral = -integral_threshold

        diff = err - self.last_err
        self.last_err = err
        pid_output = self.kp*err + beta*self.ki*self.integral + self.kd*diff
        return pid_output

class CameraCalibration:
    def __init__(self):
        self.centerX = 0.0
        self.centerY = 0.0
        self.focus = 0.0
        self.centerZ = -100

    def get_CamParam(self,centerX:int = 0, centerY: int = 0, focus : float = 0):
        '''
        units: pixel
        '''
        self.centerX = centerX
        self.centerY = centerY
        self.focus = focus

    def get_CamHigh(self, High: float = 0):
        self.High = High

    def Img2Camera(self, ImgDetX: int = 10000, ImgDetY: int = 10000)->tuple[float,float,float]:
        '''
        convert image coordination to Camera real coordination
        '''
        if(self.centerX*self.centerY*self.focus != 0 and ImgDetX<1000 and ImgDetY<1000):
            self.CameraCoordX = (ImgDetX - self.centerX) * self.High / self.focus
            self.CameraCoordY = (ImgDetY - self.centerY) * self.High / self.focus
            return self.CameraCoordX, self.CameraCoordY
        else:
            return [-1.0,-1.0]

class TransfMtx:
    def quaternion2euler(self, vehicle_odometry):
        """
        注意:此旋转顺序为ZYX顺序!!!!!!
        将四元数转换为欧拉角（roll, pitch, yaw）
        
        参数:
        vehicle_odometry: 包含四元数的对象，格式为 [q_w, q_x, q_y, q_z]

        返回:
        欧拉角，格式为 (roll, pitch, yaw)
        """
        # 提取四元数分量
        q_w = vehicle_odometry[0]
        q_x = vehicle_odometry[1]
        q_y = vehicle_odometry[2]
        q_z = vehicle_odometry[3]

        # 计算 Roll (x轴旋转)
        roll = math.atan2(2 * (q_w * q_x + q_y * q_z), 1 - 2 * (q_x**2 + q_y**2))

        # 计算 Pitch (y轴旋转)
        sinp = 2 * (q_w * q_y - q_z * q_x)
        sinp = max(-1.0, min(1.0, sinp))  # 限制范围
        pitch = math.asin(sinp)

        # 计算 Yaw (z轴旋转)
        yaw = math.atan2(2 * (q_w * q_z + q_x * q_y), 1 - 2 * (q_y**2 + q_z**2))

        return roll, pitch, yaw
    def RXaxis(self,theta: float = 0.0):
        c, s = np.cos(theta), np.sin(theta)
        R = np.array(((1, 0, 0), 
                    (0, c, -s), 
                    (0, s, c)))
        return R
    
    def RYaxis(self,theta: float = 0.0):
        c, s = np.cos(theta), np.sin(theta)
        R = np.array(((c, 0, s), 
                    (0, 1, 0), 
                    (-s, 0, c)))
        return R

    def RZaxis(self,theta: float = 0.0):
        c, s = np.cos(theta), np.sin(theta)
        R = np.array(((c, -s, 0), 
                    (s, c, 0), 
                    (0, 0, 1)))
        return R
    
    def MulMtx(self, roll:float=0.0, pitch:float=0.0, yaw:float=0.0):
        '''
        R(ZYX) or R(YPR)
        '''
        ZY = np.dot(self.RZaxis(yaw), self.RYaxis(pitch))
        ZYX = np.dot(ZY, self.RXaxis(roll))
        return ZYX